#include "stdafx.h"
#include "gp_Trsf2d.h"
#include "gp_GTrsf2d.h"
#include "gp_XY.h"
#include "gp_Mat2d.h"
#include "gp.h"

#define Nat00 NewMat.matrix[0][0]
#define Nat01 NewMat.matrix[0][1]
#define Nat02 NewMat.matrix[0][2]
#define Nat10 NewMat.matrix[1][0]
#define Nat11 NewMat.matrix[1][1]
#define Nat12 NewMat.matrix[1][2]
#define Nat20 NewMat.matrix[2][0]
#define Nat21 NewMat.matrix[2][1]
#define Nat22 NewMat.matrix[2][2]

#define Mat2d00 matrix[0][0]
#define Mat2d01 matrix[0][1]
#define Mat2d10 matrix[1][0]
#define Mat2d11 matrix[1][1]

#define Nat2d00 NewMat2d.matrix[0][0]
#define Nat2d01 NewMat2d.matrix[0][1]
#define Nat2d10 NewMat2d.matrix[1][0]
#define Nat2d11 NewMat2d.matrix[1][1]

#define Oat2d00 Other.matrix[0][0]
#define Oat2d01 Other.matrix[0][1]
#define Oat2d10 Other.matrix[1][0]
#define Oat2d11 Other.matrix[1][1]

gp_Mat2d::gp_Mat2d ()
{
	Mat2d00 = Mat2d01 = Mat2d10 = Mat2d11 = 0.0;
}

void gp_Mat2d::SetDiagonal (const double X1,
	const double X2)
{
	Mat2d00 = X1; Mat2d11 = X2;
}

void gp_Mat2d::SetIdentity ()
{
	Mat2d00 = Mat2d11 = 1.0;
	Mat2d01 = Mat2d10 = 0.0;
}

void gp_Mat2d::SetRotation (const double Ang)
{
	double SinA = sin(Ang);
	double CosA = cos(Ang);
	Mat2d00 = Mat2d11 = CosA;
	Mat2d01 = -SinA;
	Mat2d10 =  SinA;
}

void gp_Mat2d::SetScale (const double S)
{
	Mat2d00 = Mat2d11 = S;
	Mat2d01 = Mat2d10 = 0.0;
}

void gp_Mat2d::SetValue (const int Row, 
	const int Col, 
	const double Value)
{
	Standard_OutOfRange_Raise_if
	(Row < 1 || Row > 2 || Col < 1 || Col > 2, " ");
	matrix[Row-1][Col-1] = Value;
}

double gp_Mat2d::Determinant () const
{
	return  Mat2d00 * Mat2d11 - Mat2d10 * Mat2d01;
}

const double& gp_Mat2d::Value (const int Row, 
	const int Col) const
{
	Standard_OutOfRange_Raise_if
	(Row < 1 || Row > 2 || Col < 1 || Col > 2, " ");

	return matrix[Row-1][Col-1];
}

double& 
	gp_Mat2d::ChangeValue (const int Row, 
	const int Col)
{
	Standard_OutOfRange_Raise_if
	(Row < 1 || Row > 2 || Col < 1 || Col > 2, " ");
	return matrix[Row-1][Col-1];
}

bool gp_Mat2d::IsSingular () const
{
	double det = Determinant();
	if (det < 0) det = - det;
	return det <= gp::Resolution();
}

void gp_Mat2d::Add (const gp_Mat2d& Other)
{
	Mat2d00 += Oat2d00;
	Mat2d01 += Oat2d01;
	Mat2d10 += Oat2d10;
	Mat2d11 += Oat2d11;
}

gp_Mat2d gp_Mat2d::Added (const gp_Mat2d& Other) const
{
	gp_Mat2d NewMat2d;
	Nat2d00 = Mat2d00 + Oat2d00;
	Nat2d01 = Mat2d01 + Oat2d01;
	Nat2d10 = Mat2d10 + Oat2d10;
	Nat2d11 = Mat2d11 + Oat2d11;
	return NewMat2d;
}

void gp_Mat2d::Divide (const double Scalar)
{
	Mat2d00 /= Scalar;
	Mat2d01 /= Scalar;
	Mat2d10 /= Scalar;
	Mat2d11 /= Scalar;
}

gp_Mat2d gp_Mat2d::Divided (const double Scalar) const
{
	gp_Mat2d NewMat2d;
	Nat2d00 = Mat2d00 / Scalar;
	Nat2d01 = Mat2d01 / Scalar;
	Nat2d10 = Mat2d10 / Scalar;
	Nat2d11 = Mat2d11 / Scalar;
	return NewMat2d;
}

gp_Mat2d gp_Mat2d::Inverted () const
{
	gp_Mat2d NewMat = *this;
	NewMat.Invert();
	return NewMat;
}

gp_Mat2d gp_Mat2d::Multiplied (const gp_Mat2d& Other) const
{
	gp_Mat2d NewMat2d = *this;
	NewMat2d.Multiply(Other);
	return NewMat2d;
}

void gp_Mat2d::Multiply (const gp_Mat2d& Other)
{
	double T00,T10;
	T00     = Mat2d00 * Oat2d00 + Mat2d01 * Oat2d10;
	T10     = Mat2d10 * Oat2d00 + Mat2d11 * Oat2d10;
	Mat2d01 = Mat2d00 * Oat2d01 + Mat2d01 * Oat2d11;
	Mat2d11 = Mat2d10 * Oat2d01 + Mat2d11 * Oat2d11;
	Mat2d00 = T00;
	Mat2d10 = T10;
}

void gp_Mat2d::PreMultiply (const gp_Mat2d& Other)
{
	double T00,T01;
	T00     = Oat2d00 * Mat2d00 + Oat2d01 * Mat2d10;
	Mat2d10 = Oat2d10 * Mat2d00 + Oat2d11 * Mat2d10;
	T01     = Oat2d00 * Mat2d01 + Oat2d01 * Mat2d11;
	Mat2d11 = Oat2d10 * Mat2d01 + Oat2d11 * Mat2d11;
	Mat2d00 = T00;
	Mat2d01 = T01;
}

gp_Mat2d gp_Mat2d::Multiplied (const double Scalar) const
{
	gp_Mat2d NewMat2d;
	Nat2d00 = Mat2d00 * Scalar;
	Nat2d01 = Mat2d01 * Scalar;
	Nat2d10 = Mat2d10 * Scalar;
	Nat2d11 = Mat2d11 * Scalar;
	return NewMat2d;
}

void gp_Mat2d::Multiply (const double Scalar)
{
	Mat2d00 *= Scalar;
	Mat2d01 *= Scalar;
	Mat2d10 *= Scalar;
	Mat2d11 *= Scalar;
}

gp_Mat2d gp_Mat2d::Powered (const int N) const
{
	gp_Mat2d Mat2dN = *this;
	Mat2dN.Power (N);
	return Mat2dN;
}

void gp_Mat2d::Subtract (const gp_Mat2d& Other)
{
	Mat2d00 -= Oat2d00;
	Mat2d01 -= Oat2d01;
	Mat2d10 -= Oat2d10;
	Mat2d11 -= Oat2d11;
}

gp_Mat2d gp_Mat2d::Subtracted (const gp_Mat2d& Other) const
{
	gp_Mat2d NewMat2d;
	Nat2d00 = Mat2d00 - Oat2d00;
	Nat2d01 = Mat2d01 - Oat2d01;
	Nat2d10 = Mat2d10 - Oat2d10;
	Nat2d11 = Mat2d11 - Oat2d11;
	return NewMat2d;
}

void gp_Mat2d::Transpose ()
{
	double Temp;
	Temp     = Mat2d01;
	Mat2d01  = Mat2d10;
	Mat2d10  = Temp;
}

gp_Mat2d gp_Mat2d::Transposed () const
{
	gp_Mat2d NewMat2d;
	Nat2d10 = Mat2d01;
	Nat2d01 = Mat2d10;
	Nat2d00 = Mat2d00;
	Nat2d11 = Mat2d11;
	return NewMat2d; 
}

gp_Mat2d operator* (const double Scalar,
	const gp_Mat2d& Mat2D)
{ return Mat2D.Multiplied (Scalar); }

gp_Mat2d::gp_Mat2d (const gp_XY& Col1, const gp_XY& Col2)
{
  Mat2d00 = Col1.X(); Mat2d10 = Col1.Y();
  Mat2d01 = Col2.X(); Mat2d11 = Col2.Y();
}

void gp_Mat2d::SetCol (const int Col,
		       const gp_XY& Value)
{
  if(Col < 1 || Col > 2) return;
  if  (Col == 1) {
    Mat2d00 = Value.X();
    Mat2d10 = Value.Y();
  }
  else {
    Mat2d01 = Value.X();
    Mat2d11 = Value.Y();
  }
}

void gp_Mat2d::SetCols (const gp_XY& Col1,
			const gp_XY& Col2)
{
  Mat2d00 = Col1.X(); Mat2d10 = Col1.Y();
  Mat2d01 = Col2.X(); Mat2d11 = Col2.Y();
}

void gp_Mat2d::SetRow (const int Row, const gp_XY& Value)
{
  if(Row < 1 || Row > 2) return;
  if (Row == 1) {
    Mat2d00 = Value.X();
    Mat2d01 = Value.Y();
  }
  else {
    Mat2d10 = Value.X();
    Mat2d11 = Value.Y();
  }
}

void gp_Mat2d::SetRows (const gp_XY& Row1, const gp_XY& Row2)
{
  Mat2d00 = Row1.X(); Mat2d01 = Row1.Y();
  Mat2d10 = Row2.X(); Mat2d11 = Row2.Y();
}

gp_XY gp_Mat2d::Column (const int Col) const
{
  Standard_OutOfRange_Raise_if
  (Col < 1 || Col > 2, " ");
  if (Col == 1) return gp_XY (Mat2d00,Mat2d10);
  return gp_XY (Mat2d01,Mat2d11);
}

gp_XY gp_Mat2d::Diagonal () const
{ 
  return gp_XY (Mat2d00,Mat2d11);
}

gp_XY gp_Mat2d::Row (const int Row) const
{
  Standard_OutOfRange_Raise_if
  (Row < 1 || Row > 2, " ");
  if (Row == 1) return gp_XY (Mat2d00,Mat2d01);
  return gp_XY (Mat2d10,Mat2d11);
}

void gp_Mat2d::Invert ()
{
  double det ;
  gp_Mat2d NewMat;
  Nat00 = Mat2d11 ;
  Nat01 = -Mat2d01 ;
  Nat10 = -Mat2d10 ;
  Nat11 = Mat2d00  ;
  det = Nat00 * Nat11 - Nat01 * Nat10 ;
  double val = det;
  if (val < 0) val = - val;
  if(val <= gp::Resolution()) return;
  det = 1.0 / det ;
  Mat2d00 = Nat00 * det ;
  Mat2d10 = Nat10 * det ;
  Mat2d01 = Nat01 * det ;
  Mat2d11 = Nat11 * det ;
}

void gp_Mat2d::Power (const int N)
{
  if      (N ==  1) { }
  else if (N ==  0) { SetIdentity (); }
  else if (N == -1) { Invert(); }
  else {
    if (N < 0) Invert();
    int Npower = N;
    if (Npower < 0) Npower = - Npower;
    Npower--;
    gp_Mat2d Temp = *this;
    for(;;) {
      if (IsOdd(Npower)) Multiply (Temp);
      if (Npower == 1)   break;
      Temp.Multiply (Temp);
      Npower = Npower/2;
    }
  }
}

